#include "Tools.h"

/**
 * @brief 四元素转欧拉角
 * @param orientation-四元素
 * @return 欧拉角
 * */
float Tools::QuaternionTransToYaw(geometry_msgs::Quaternion orientation)
{
    tf::Quaternion quat;
    tf::quaternionMsgToTF(orientation, quat);

    double roll, pitch, yaw;
    tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);

    return (float)yaw;
}